
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Import/RobotImporterFactory.h>

#include <Inventor/Qt/SoQt.h>
#include <QFileInfo>

#include <boost/extension/shared_library.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <string>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderC3D.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessor.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>


using std::cout;
using std::endl;
using namespace VirtualRobot;

int main(int argc, char *argv[])
{
    cout << " --- MMM motion cutter --- " << endl;
    cout << "cut one mmm motion file into a shorter mmm motion file according to two frame numbers" << endl;

    // we are expecting three arguments, and that is the filename of the file to convert
    if (argc<4)
    {
        cout << "Missing filename as first argument, aborting..." << endl;
        cout << "Syntax: cutmmm %motionfile_in %cutframe_num1 %cutframe_num2 %targetmotionfile_out" << endl;
        //MMM_ERROR << endl << "Could not process command line, aborting..." << endl;
        return -1;
    }
    //c.print();


    std::string filename = std::string(argv[1]);

    int frameNum1, frameNum2;
    std::istringstream start(argv[2]);
    start >> frameNum1;
    std::istringstream end(argv[3]);
    end >> frameNum2;

    std::string filenameOut = std::string(argv[4]);

    cout << "Filenames provided for cutting: [" << filename << "] -> [" << filenameOut << "]" << endl;

    if (!filename.empty())
    {
        // input file 1
        QFileInfo fileInfo(filename.c_str());
        std::string path(fileInfo.path().toLatin1());
        std::string file(fileInfo.fileName().toLatin1());
        std::string suffix(fileInfo.suffix().toLatin1());

        // transform suffixes to lowerCase
        std::transform(suffix.begin(), suffix.end(), suffix.begin(), ::tolower);

        // For now, we just concatenate MMM motion files, so we check for correct suffixes
        if ((suffix.compare("xml")!=0))
        {
            cout << "Expected MMM Motion files as input (.xml). Aborting..." << endl;
            return -1;
        }
        // load input files
        MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
        cout << "Checking motions in file: " << endl;
        std::vector < std::string > motionNames = r->getMotionNames(filename);

        // there should be at least one motion in every file
        if (motionNames.size() == 0)
        {
            MMM_ERROR << "no motions in file!" << endl;
            return -1;
        }
        else
            cout << "Number of Motions in file:" << motionNames.size() << endl;

        // save maximum frame numbers over all motions in this integer
        int frameCount = -1;
        // save motions in a motion list for faster access
        MMM::LegacyMotionList motions;
        // find the maximum frame number by iterating over all motions
        for(size_t i = 0; i < motionNames.size(); i++)
        {
            MMM::LegacyMotionPtr motion = r->loadMotion(filename,motionNames[i]);
            // skip empty motions
            if(!motion)
            {
                cout << "skipping motion [" << motionNames[i] << "]" << endl;
                continue;
            }
            motions.push_back(motion);
            cout << "Checking motion: [" << motion->getName() << "]";
            if(frameCount == -1 || frameCount < (int) motion->getNumFrames())
            {
                frameCount = motion->getNumFrames();
                cout << "\tSetting maximum frameCount to " << frameCount << endl;
            }
            cout << "... Number of frames: [" << motion->getNumFrames() << "]" << endl;
            cout  << "\tcontaining model file: [" << (motion->getModel() ? motion->getModel()->getFilename() : "unknown") << "]" << endl;
        }

        cout << "Number of total frames: [" << frameCount << "]" << endl;
        cout << "Number of total motions: [" << motions.size() << "]" << endl;

        // Frame number checker
        if (frameCount < frameNum2)
        {
            MMM_ERROR << "The number of frames is smaller than required " << frameNum2 << endl;
            return -1;
        }


        std::vector< MMM::LegacyMotionPtr > cuttedMotions;

        // Expand every motion to the max frame size
        // Cut every motion according to two frame numbers
        for (unsigned int i=0; i<motions.size(); i++)
        {
            // Step1: Expand every motion
            MMM::LegacyMotionPtr motion = motions[i];
            // calculate missing motion frames
            int delta = frameCount - motion->getNumFrames();
            // if no frames are missing, continue with next motion
            if (delta != 0)
            {
                // otherwise duplicate the last frame j-times, so that the frame count matches
                MMM::MotionFramePtr frame = motion->getMotionFrame(motion->getNumFrames()-1);
                // predefined timestep
                float dt = 0.01f;
                float time = frame->timestep;
                cout << "adding [" << delta << "] frames to the motion [" << motion->getName() << "]" << " using frame at timestep ["<< time << "]" << endl;
                // if there are at least 2 motion frames, calculate the timestep delta, otherwise use the predefined value
                if (motion->getNumFrames()>1)
                {
                    dt = motion->getMotionFrame(1)->timestep - motion->getMotionFrame(0)->timestep;
                    cout << "\tcalculated delta-time: [" << dt << "]" << endl;
                } else
                    cout << "\tpre-defined delta-time: [" << dt << "]" << endl;
                MMM::MotionFramePtr f;
                for (int j=1; j<delta+1; j++)
                {
                    // make a copy of frame last frame
                    f.reset(new MMM::MotionFrame(*frame));
                    // and only change the timestep property
                    f->timestep = time + dt*j;
                    // add frame to motion
                    motion->addMotionFrame(f);
                }
            }

            // Step2: Cut motion

            MMM::LegacyMotionPtr cuttedMotion = motion->getSegmentMotion(frameNum1, frameNum2);
            cuttedMotion->setStartTime(0);
            cuttedMotions.push_back(cuttedMotion);
        }


        // xml-string for all motions
        std::string content;
        // xml-string for each motion
        std::string contentMotion;
        // concatenate the xml-strings from every motion to a single xml-string
        for (unsigned int i=0; i<cuttedMotions.size(); i++)
        {
            contentMotion = cuttedMotions[i]->toXML();
            content += contentMotion;
        }
        // save this xml-string to the output file
        if (!MMM::XML::saveXML(filenameOut, content))
        {
            MMM_ERROR << " Could not write to file " << filenameOut << endl;
        }
    }
    return 0;
}
